#!/usr/bin/env python3

import rospy
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image

def image_callback(img_msg):
    bridge = CvBridge()
    try:
        # 将 ROS 图像消息转换为 OpenCV 图像
        cv_image = bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8')
    except CvBridgeError as e:
        rospy.logerr(f'Error converting image: {e}')
        return

    # 显示图像
    cv2.imshow("Received Image", cv_image)
    cv2.waitKey(1)

def main():
    # 初始化 ROS 节点
    rospy.init_node('image_subscriber', anonymous=True)

    # 创建图像订阅者
    rospy.Subscriber('camera/image_raw', Image, image_callback)

    # 保持节点运行
    rospy.spin()

    # 关闭 OpenCV 窗口
    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()
